#include <iostream>
#include <opencv2/opencv.hpp>
#include <numeric>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include "windmill.hpp"
#include <vector>

using namespace cv;
using namespace cv::ml;
using namespace std;

//获取点间距离
double getDistance(Point A,Point B)
{
    double dis;
    dis=pow((A.x-B.x),2)+pow((A.y-B.y),2);
    return sqrt(dis);
}

/*
 * 通过最小二乘法来拟合圆的信息
 * pts: 所有点坐标
 * center: 得到的圆心坐标
 * radius: 圆的半径
 */
static bool CircleInfo2(std::vector<cv::Point2f>& pts, cv::Point2f& center, float& radius)
{
    center = cv::Point2d(0, 0);
    radius = 0.0;
    if (pts.size() < 3) return false;;

    double sumX = 0.0;
    double sumY = 0.0;
    double sumX2 = 0.0;
    double sumY2 = 0.0;
    double sumX3 = 0.0;
    double sumY3 = 0.0;
    double sumXY = 0.0;
    double sumX1Y2 = 0.0;
    double sumX2Y1 = 0.0;
    const double N = (double)pts.size();
    for (int i = 0; i < pts.size(); ++i)
    {
        double x = pts.at(i).x;
        double y = pts.at(i).y;
        double x2 = x * x;
        double y2 = y * y;
        double x3 = x2 *x;
        double y3 = y2 *y;
        double xy = x * y;
        double x1y2 = x * y2;
        double x2y1 = x2 * y;

        sumX += x;
        sumY += y;
        sumX2 += x2;
        sumY2 += y2;
        sumX3 += x3;
        sumY3 += y3;
        sumXY += xy;
        sumX1Y2 += x1y2;
        sumX2Y1 += x2y1;
    }
    double C = N * sumX2 - sumX * sumX;
    double D = N * sumXY - sumX * sumY;
    double E = N * sumX3 + N * sumX1Y2 - (sumX2 + sumY2) * sumX;
    double G = N * sumY2 - sumY * sumY;
    double H = N * sumX2Y1 + N * sumY3 - (sumX2 + sumY2) * sumY;

    double denominator = C * G - D * D;
    if (std::abs(denominator) < DBL_EPSILON) return false;
    double a = (H * D - E * G) / (denominator);
    denominator = D * D - G * C;
    if (std::abs(denominator) < DBL_EPSILON) return false;
    double b = (H * C - E * D) / (denominator);
    double c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

    center.x = a / (-2);
    center.y = b / (-2);
    radius = std::sqrt(a * a + b * b - 4 * c) / 2;
    return true;
}

static bool Yuce(std::vector<cv::Point2f>& xiangwei, float& cita, float time)
{
    if (xiangwei.size() == 3)
    {
        return false;
    }
    
    float a,b,c,d;  //三个系数,初始值用参考值
    a = 0.785;
    b = 1.884;
    c = 1.305;
    d = 0;
    
    float xuexilv = 0.000001;  //步长
    float piancha;

    for (int i = 0; i < xiangwei.size(); i++)
    {
        float t = xiangwei[i].x;
        float cigma = xiangwei[i].y;
        a = a - xuexilv*(a*sin(b*t+d)+c-cigma)*sin(b*t+d);
        b = b - xuexilv*(a*sin(b*t+d)+c-cigma)*a*t*cos(b*t+d);
        c = c - xuexilv*(a*sin(b*t+d)+c-cigma);
        d = d - 2*(a*sin(b*t+d)+c-cigma)*a*cos(b*t+d);
    }
    
    cita = a*sin(b*time+d) + c;

    return true;
}

//模板匹配
double TemplateMatch(cv::Mat image, cv::Mat tepl, cv::Point &point, int method)
{
    int result_cols = image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;

    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method );

    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

    switch(method)
    {
    case CV_TM_SQDIFF:
    case CV_TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;

    default:
        point = maxLoc;
        return maxVal;

    }
}


int main(int argc, char *argv[])
{
    vector<Point2f> cirV;

    Point2f cc=Point2f(0,0);

    Point2f lastone = Point2f(0,0);

    //标准时间
    float timecount = std::chrono::system_clock::now().time_since_epoch().count() / 1e9;

    float lasttime = (std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount;

    vector<Point2f> xiangwei;

    WINDMILL::WindMill
    wm(std::chrono::system_clock::now().time_since_epoch().count() / 1e9);
    do
    {
    cv::Mat show =
    wm.getMat(std::chrono::system_clock::now().time_since_epoch().count() / 1e9);

    Mat srcImage;
    srcImage = show;

    Mat templ[9];
    for(int i=1;i<=8;i++)
    {
        templ[i]=imread("../template/template"+to_string(i)+".jpg",IMREAD_GRAYSCALE);
    }

    //分割颜色通道
    vector<Mat> imgChannels;
    split(srcImage,imgChannels);
    
    //获得目标颜色图像的二值图
    Mat midImage2=imgChannels.at(2)-imgChannels.at(0);

    //二值化，背景为黑色，图案为白色
    //用于查找扇叶
    threshold(midImage2,midImage2,100,255,CV_THRESH_BINARY);

    //查找轮廓
    vector<vector<Point>> contours2;
    vector<Vec4i> hierarchy2;
    findContours(midImage2,contours2,hierarchy2,CV_RETR_TREE,CHAIN_APPROX_SIMPLE);

    RotatedRect rect_tmp2;
    bool findTarget=0;

    //遍历轮廓
    if(hierarchy2.size())
    {
        for(int i=0;i>=0;i=hierarchy2[i][0])
        {
            rect_tmp2=minAreaRect(contours2[i]);
            Point2f P[4];
            rect_tmp2.points(P);

            Point2f srcRect[4];
            Point2f dstRect[4];

            double width;
            double height;

            //矫正提取的叶片的宽高
            width=getDistance(P[0],P[1]);
            height=getDistance(P[1],P[2]);
            if(width>height)
            {
                srcRect[0]=P[0];
                srcRect[1]=P[1];
                srcRect[2]=P[2];
                srcRect[3]=P[3];
            }
            else
            {
                swap(width,height);
                srcRect[0]=P[1];
                srcRect[1]=P[2];
                srcRect[2]=P[3];
                srcRect[3]=P[0];
            }
            //通过面积筛选
            double area=height*width;
            if(area>5000)
            {
                dstRect[0]=Point2f(0,0);
                dstRect[1]=Point2f(width,0);
                dstRect[2]=Point2f(width,height);
                dstRect[3]=Point2f(0,height);

                // 应用透视变换，矫正成规则矩形
                Mat transform = getPerspectiveTransform(srcRect,dstRect);
                Mat perspectMat;
                warpPerspective(midImage2,perspectMat,transform,midImage2.size());

                // 提取扇叶图片
                Mat testim;
                testim = perspectMat(Rect(0,0,width,height));

                if(testim.empty())
                {
                    cout<<"filed open"<<endl;
                    return -1;
                }

                //模版匹配法
                cv::Point matchLoc;
                double value;
                Mat tmp1;
                resize(testim,tmp1,Size(42,20));

                vector<double> Vvalue1;
                vector<double> Vvalue2;
                for(int j=1;j<=6;j++)
                {
                    value = TemplateMatch(tmp1, templ[j], matchLoc, CV_TM_CCOEFF_NORMED);
                    Vvalue1.push_back(value);
                }
                for(int j=7;j<=8;j++)
                {
                    value = TemplateMatch(tmp1, templ[j], matchLoc, CV_TM_CCOEFF_NORMED);
                    Vvalue2.push_back(value);
                }

                int maxv1=0,maxv2=0;
                for(int t1=0;t1<6;t1++)
                {
                    if(Vvalue1[t1]>Vvalue1[maxv1])
                    {
                        maxv1=t1;
                    }
                }
                for(int t2=0;t2<2;t2++)
                {
                    if(Vvalue2[t2]>Vvalue2[maxv2])
                    {
                        maxv2=t2;
                    }
                }

                //预测是否是要打击的扇叶
                if(Vvalue1[maxv1]>Vvalue2[maxv2]&&Vvalue1[maxv1]>0.6)
                {
                    findTarget=true;
                    //查找装甲板
                    if(hierarchy2[i][2]>=0)
                    {
                        RotatedRect rect_tmp=minAreaRect(contours2[hierarchy2[i][2]]);
                        Point2f Pnt[4];
                        rect_tmp.points(Pnt);
                        const float maxHWRatio=0.7153846;
                        const float maxArea=2000;
                        const float minArea=500;

                        float width=rect_tmp.size.width;
                        float height=rect_tmp.size.height;
                        if(height>width)
                        {
                            swap(height,width);
                        }
                            
                        float area=width*height;
                        if(height/width>maxHWRatio||area>maxArea ||area<minArea)
                        {
                            continue;
                        }

                        Point centerP=rect_tmp.center;
                        //打击点
                        circle(srcImage,centerP,10,Scalar(0,255,0),-1);

                        //用于拟合圆，用90个点拟合圆
                        if(cirV.size()<90)
                        {
                            cirV.push_back(centerP);
                        }
                        else
                        {
                            float R;
                            //得到拟合的圆心
                            CircleInfo2(cirV,cc,R);

                            cirV.erase(cirV.begin());
                        }
                        if(cc.x!=0&&cc.y!=0)
                        {
                            float xx=-(cc.x-centerP.x);
                            float yy=-(cc.y-centerP.y);

                            float delta = (fastAtan2(lastone.y,lastone.x) - fastAtan2(-yy,-xx))*3.14159/180;
                            //拟合下一次读入图像用下面这个
                            //Point2f nihe = Point2f((std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount,delta/((std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount-lasttime));
                            Point2f nihe = Point2f((std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount,delta);
                            
                            if(xiangwei.size()<90)
                            {
                                xiangwei.push_back(nihe);
                            }
                            else
                            {
                                float cita;
                                //得到预测的目标点
                                Yuce(xiangwei,cita,(std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount);

                                //拟合下一次读入图像用下面这个
                                //Mat rot_mat=getRotationMatrix2D(cc,180*cita*((std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount-lasttime)/3.14159,1);
                                Mat rot_mat=getRotationMatrix2D(cc,180*cita/3.14159,1);

                                float sinA=rot_mat.at<double>(0,1);
                                float cosA=rot_mat.at<double>(0,0);
                            
                                Point2f resPoint=Point2f(cc.x+cosA*xx+sinA*yy,cc.y-sinA*xx+cosA*yy);
                                circle(srcImage,resPoint,10,Scalar(255,0,0),-1);

                                if(xiangwei.size()>90)
                                {
                                    xiangwei.erase(xiangwei.begin());
                                } 
                            }

                            lasttime = (std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - timecount;
                            Point2f lastone = Point2f(-xx,-yy);
                        }
                    }
                }
            }
        }
    }
    cv::imshow("show", show);

    } while (cv::waitKey(1) != 'q');

    return 0;
}
